#include "motorstatesensor.h"
#include <QDebug>


int MotorStateSensor::counter=0;

MotorStateSensor::MotorStateSensor(AbstractSystem *a_sys, int a_id, int freq, double a_delay, double a_imprecision):
    sys(a_sys), id(a_id), imprecision(a_imprecision)
{
    dt = 1./freq;

    delay = static_cast<int>(a_delay*freq);
    for(int i=0; i<delay; i++)
    {
        storedValues.append(0);
    }

    timer = new QTimer;
    timer->setInterval(static_cast<int>(1000*dt));
    timer->start();

    value = 0;

    connect(timer,SIGNAL(timeout()), this, SLOT(update()));
}

void MotorStateSensor::update()
{
    double newValue = sys->getMotorStateNormee(id);
    //TODO gaussian noise on result
    if(delay >0){
        value = storedValues[counter];
        storedValues[counter] = newValue;
        counter= (counter+1)%delay;
    }
    else{
        value = newValue;
    }

}

double MotorStateSensor::getValue()
{
    return(value);
}
